#ifndef MATRIX_H_
#define MATRIX_H_

#include "Fixed.h"

namespace uav
{
namespace math
{

/**
 * @class Matrix
 *
 * @brief 3x3 fixed-point matrix class.
 *
 * @author Travis Portz
 */
class Matrix
{
public:
	Fixed m11, m12, m13,
	      m21, m22, m23,
	      m31, m32, m33;

	/**
	 * Constructs a zero matrix.
	 */

	Matrix() {}
	/**
	 * Constructs a matrix with the given elements.
	 */
	Matrix(const Fixed &m11, const Fixed &m12, const Fixed &m13,
	       const Fixed &m21, const Fixed &m22, const Fixed &m23,
	       const Fixed &m31, const Fixed &m32, const Fixed &m33)
			: m11(m11), m12(m12), m13(m13),
			  m21(m21), m22(m22), m23(m23),
			  m31(m31), m32(m32), m33(m33) {}

	/**
	 * Computes the determinant of the matrix.
	 */
	Fixed determinant() const;

	/**
	 * Computes the inverse of the matrix.
	 */
	Matrix inverse() const;

	/**
	 * Returns the transpose of the matrix.
	 */
	Matrix transpose() const
	{
		return Matrix(m11, m21, m31,
		              m12, m22, m32,
		              m13, m23, m33);
	}

	static const Matrix Identity; ///< The identity matrix.

	Matrix operator*(const Matrix &m) const;
	Matrix& operator*=(const Matrix &m);

	Matrix& operator+=(const Matrix &m);
	Matrix& operator-=(const Matrix &m);
	Matrix& operator*=(const Fixed &s);
	Matrix& operator/=(const Fixed &s);

	Matrix operator+(const Matrix &m) const
	{
		Matrix temp = *this;
		temp += m;
		return temp;
	}
	Matrix operator-(const Matrix &m) const
	{
		Matrix temp = *this;
		temp -= m;
		return temp;
	}
	Matrix operator*(const Fixed &s) const
	{
		Matrix temp = *this;
		temp *= s;
		return temp;
	}
	Matrix operator/(const Fixed &s) const
	{
		Matrix temp = *this;
		temp /= s;
		return temp;
	}
	friend Matrix operator*(const Fixed &s, const Matrix &m)
	{
		Matrix temp = m;
		temp *= s;
		return temp;
	}

	Matrix operator-() const
	{
		return Matrix(-m11, -m21, -m31,
					  -m12, -m22, -m32,
					  -m13, -m23, -m33);
	}

private:
	Fixed cofactor11() const { return (m22 * m33 - m23 * m32); }
	Fixed cofactor12() const { return (m23 * m31 - m21 * m33); }
	Fixed cofactor13() const { return (m21 * m32 - m22 * m31); }
	Fixed cofactor21() const { return (m13 * m32 - m12 * m33); }
	Fixed cofactor22() const { return (m11 * m33 - m13 * m31); }
	Fixed cofactor23() const { return (m12 * m31 - m11 * m32); }
	Fixed cofactor31() const { return (m12 * m23 - m13 * m22); }
	Fixed cofactor32() const { return (m13 * m21 - m11 * m23); }
	Fixed cofactor33() const { return (m11 * m22 - m12 * m21); }
};

}}

#endif
